/**
 * This class contains code for controlling roomba via a touch screen interface. This is for testing purpose. 
 */

package com.mcp_31091.roomba.Views;

import static com.mcp_31091.roomba.RoombaActivity.btThread;
import java.util.List;
import android.app.Activity;
import android.content.Context;
import android.net.wifi.ScanResult;
import android.net.wifi.WifiManager;
import android.os.Bundle;
import android.util.Log;
import android.view.View;
import android.widget.Button;
import android.widget.EditText;
import android.widget.TextView;

import com.mcp_31091.R;
import com.mcp_31091.roomba.helper.Helper;

public class RoombaDriveView extends Activity {
	public static final String TAG = "DebugInfo";
	protected static final char CMD_MOVE_FORWARD = 0x01;
	protected static final char CMD_MOVE_BACKWARD = 0x02;
	protected static final char CMD_SPIN_LEFT = 0x03;
	protected static final char CMD_SPIN_RIGHT = 0x04;
	protected static final char CMD_SEND_WIFI = 0x05;
	private Button driveForwardButton;
	private Button driveBackwardButton;
	private int yPos = 0, xPos = 0; // because the x and y coordinates are in
									// milli-meters, just use integer should be fine
//	private float turningRadius = 0;

	private double orientation;
	private WifiManager wifi;
	private Button spinLeftButton;
	private Button spinRightButton;
	private EditText driveForwardText;
	private EditText driveBackwardText;
	private Button setSpeedButton;
	private EditText setSpeedText;
	private Button resetButton;
	private Button showInfoButton;
	private TextView showInfoText;
	private Button initButton;
	private EditText initXText;
	private EditText initYText;
	private EditText initOText;
	private int counter = 0;
	private int preX = 0;
	private int preY = 0;
	private Button sendWifiButton;
	private Button spinLeftAtButton;
	private EditText spinLeftAtText;
//	private Button driveButton;

	@Override
	protected void onCreate(Bundle savedInstanceState) {
		super.onCreate(savedInstanceState);
		setContentView(R.layout.test_drive_panel);
		setupViews();
		wifi = (WifiManager) getSystemService(Context.WIFI_SERVICE);
	}

	/* Set driving speed of roomba */
	private void setSpeed(int speed) {
		btThread.speed = speed;
	}

	/*Go forward or backward a certain amount of distance*/
	public void goStraight(int distance) {
		int pausetime = Math.abs(distance * 1000 / btThread.speed); // mm/(mm/sec)
																	// = sec
		int remainder = pausetime % 1000;

		if (distance > 0) // forward
		{
			btThread.goStraightAt(btThread.speed);
			for (int i = 0; i < pausetime / 1000; i++) {
				btThread.pause(1000);
				updatePos(btThread.speed);
			}
		} else // backward
		{
			btThread.goStraightAt(-btThread.speed);
			for (int i = 0; i < pausetime / 1000; i++) {
				btThread.pause(1000);
				updatePos(-1 * btThread.speed);
			}
		}

		if (remainder != 0) {
			btThread.pause(remainder);
//			System.out.println("remainder*distance/time is: " + remainder
//					* distance / pausetime);
			updatePos(remainder * distance / pausetime);
		}
		btThread.stopDrive();
	}

	
	/* Update the current posistion after driving a certain amount of distance*/
	private void updatePos(int distance) {
		System.out.println("distance is " + distance);
		System.out.println("xPos begin" + xPos);
		xPos += (int) (distance * Math.cos(orientation * 2 / 360 * Math.PI));
		yPos += (int) (distance * Math.sin(orientation * 2 / 360 * Math.PI));
		System.out
				.println("(int) (distance * Math.cos(orientation*2/360*Math.PI)) is"
						+ (int) (distance * Math.cos(orientation * 2 / 360
								* Math.PI)));
		System.out.println("xPos " + xPos);
	}

	/*Update orientation and coordinates  */
	public void move(double turn, int forward) {
		if (forward == 0)
			return;
		if (turn != 0)
			spinCalculation(turn);
		// if(forward > 0)
		// btThread.goForward(forward);
		// if(forward < 0)
		// {
		// btThread.goBackward(-1*forward);
		// // Toast.makeText(this, "going backwards", Toast.LENGTH_LONG).show();
		// }
		// btThread.stopDrive();

		orientation += turn;// + new Random().nextGaussian()*turn_noise;
		orientation %= 360;
		int distance = forward; // + new Random().nextGaussian()*forward_noise;
		xPos += (int) (distance * Math.cos(orientation * 2 / 360 * Math.PI));
		// Toast.makeText(this, "Math.cos(orientation*2/360*Math.PI) is " +
		// Math.cos(orientation*2/360*Math.PI), Toast.LENGTH_LONG).show();
		System.out.println("xpos Math.cos(orientation*2/360*Math.PI) is "
				+ (int) Math.cos(orientation * 2 / 360 * Math.PI));
		yPos += (int) (distance * Math.sin(orientation * 2 / 360 * Math.PI));
		System.out.println("ypos Math.cos(orientation*2/360*Math.PI) is "
				+ (int) Math.sin(orientation * 2 / 360 * Math.PI));
	}

	void setupViews() {
		setSpeed(150); // a little fast
		// spinButton = (Button)findViewById(R.id.spin_button);
		sendWifiButton = (Button) findViewById(R.id.send_wifi_button);
//		driveButton = (Button) findViewById(R.id.drive_button);
		
		setSpeedButton = (Button) findViewById(R.id.set_speed_button);
		setSpeedText = (EditText) findViewById(R.id.set_speed_text);

		driveForwardButton = (Button) findViewById(R.id.drive_forward_button);
		driveForwardText = (EditText) findViewById(R.id.drive_forward_text);
		driveForwardText.setText("500");

		driveBackwardButton = (Button) findViewById(R.id.drive_backward_button);
		driveBackwardText = (EditText) findViewById(R.id.drive_backward_text);
		driveBackwardText.setText("500");

		spinLeftButton = (Button) findViewById(R.id.spin_left_button);
		spinRightButton = (Button) findViewById(R.id.spin_right_button);

		showInfoButton = (Button) findViewById(R.id.show_info_button);
		showInfoText = (TextView) findViewById(R.id.info_text);

		spinLeftAtButton = (Button) findViewById(R.id.spin_left_at_button);
		spinLeftAtText = (EditText) findViewById(R.id.spin_left_at_text);

		initButton = (Button) findViewById(R.id.init_button);
		initXText = (EditText) findViewById(R.id.init_x_text);
		initYText = (EditText) findViewById(R.id.init_y_text);
		initOText = (EditText) findViewById(R.id.init_o_text);

		resetButton = (Button) findViewById(R.id.reset_button);

		/*Set driving speed */
		setSpeedButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				setSpeed(Integer.parseInt(setSpeedText.getText().toString()));
			}
		});
		
		/* Thread to send a x,y coordnates */
		class SendCordThread extends Thread {
			public void run() {
				while (!isInterrupted()) {
					for (int i = 0; i < 10000; i++) // delay some time
					{
						if (i == 3000) {
							System.out.println(i);
						}
					}

					if ((preX != xPos) || (preY != yPos)) // if a different
															// point
					{
						sendCords();
						preX = xPos;
						preY = yPos;
					}
				}
				sendCords();
			}
		};

		/*Drive forward button*/
		driveForwardButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				// move(0,
				// Integer.parseInt(driveForwardText.getText().toString()));
				final SendCordThread sendCordThread = new SendCordThread();
				sendCordThread.start();
				int forwardDistance = Integer.parseInt(driveForwardText
						.getText().toString());
				btThread.newDrive(forwardDistance);
				move(0, forwardDistance);
				sendCordThread.interrupt();
			}
		});

		/*Drive backward button*/
		driveBackwardButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				final SendCordThread backwardThread = new SendCordThread();
				backwardThread.start();
				int backwardDistance = Integer.parseInt(driveBackwardText
						.getText().toString());
				btThread.newDrive(-1 * backwardDistance);
				move(0, -1 * backwardDistance);
				backwardThread.interrupt();
			}
		});
		
		/*Spin left 90 degree button*/
		spinLeftButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				spinLeft();
			}
		});
		
		/*Spin right 90 degree button*/
		spinRightButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				spinRight();
			}
		});

		/*Spin left a certain degree */
		spinLeftAtButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				System.out.println("spined");
				System.out.println(Integer.parseInt(spinLeftAtText.getText()
						.toString()));

				btThread.newTurn(Integer.parseInt(spinLeftAtText.getText()
						.toString()));
			}
		});

		/*Show current x,y coordinates */
		showInfoButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				showInfoText.setText("counter: " + counter + "x:" + xPos
						+ " y:" + yPos + " orientatioin:" + orientation);
			}
		});

		/*Set initial x,y,orientation value*/
		initButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				xPos = Integer.parseInt(initXText.getText().toString());
				yPos = Integer.parseInt(initYText.getText().toString());
				orientation = Integer.parseInt(initOText.getText().toString());
			}
		});

		/*Reset button*/
		resetButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				reset();
			}
		});

		/*Send wifi signal button*/
		sendWifiButton.setOnClickListener(new View.OnClickListener() {
			public void onClick(View v) {
				new Thread(new Runnable() {
					public void run() {
						sendWifiSignal();
					}
				}).start();
			}
		});
	}

	/*Reset orientation, x, y*/
	void reset() {
		orientation = 0;
		xPos = 0;
		yPos = 0;
		counter = 0;
	}

	/*Spin left 90 degree, roomba is lightly off, 85*/
	void spinLeft() {
		// btThread.spinLeft(90);
		btThread.newTurn(85);
		orientation += 90;// + new Random().nextGaussian()*turn_noise;
		orientation %= 360;
	}

	/*Spin right 90 degree*/
	void spinRight() {
		// btThread.spinRight(90);
		btThread.newTurn(-85);
		orientation += 360;
		orientation -= 90;
		orientation %= 360;
	}
	
	/*Send wifi signal to server*/
	private void sendWifiSignal() {
		String signal = "couter," + counter + ",x," + xPos + ",y," + yPos
				+ ", ";
		wifi.startScan();
		List<ScanResult> scanResults = wifi.getScanResults();
		for (ScanResult sr : scanResults) {
			Log.i("SignalTag", sr.toString());
			signal += sr.BSSID + "," + sr.level;
		}
		Helper.sendPacket(signal); // send one point of signals at once
		counter++;
	}
	
	/*Send x, y coordinates to server*/
	private void sendCords() {
		Helper.sendPacket("x:," + xPos + ",y:," + yPos);
	}

	/*calculate for spin*/
	public void spinCalculation(double degree) {
		float pausetime = 100 / 1000 + (float) Math
				.abs(((float) (230 * Math.PI / 360.0)) * 103.8 / 100);
		int realpause = (int) (pausetime * 1000 * degree / 90);
		btThread.drive(100, 1);
		btThread.pause(realpause);
		btThread.stopDrive();
	}

//	public void spin(View view) {
//		orientation = 0;
//		xPos = 0;
//		yPos = 0;
//		for (int i = 0; i < 12; i++) {
//			move(0, 500);
//			new Thread(new Runnable() {
//				public void run() {
//					sendWifiSignal();
//				}
//			}).start();
//		}
//	}
}
